#include "vfx_softbody.h"

#include "math/intersection.h"
#include "graphics/graphics_util.h"
#include "audio/audio.h"

#include "dd/dd_man.h"
#include "util/mmgr.h"		    // Needs to come in early
#include "util/common_macros.h"		// macros
#include "util/log.h"		    // logging
#include "util/timer.h"

#include <cassert>
#include <math.h>

using namespace vfx;

SFXSoftbody::SFXSoftbody() :
	m_DefaultSpring(3), m_DefaultMass(1), m_DefaultGravity(1),
	m_Width(1), m_Height(1),
	m_NodeA(NULL),
	m_Color(0xFFFFFFFF),
	m_IsFlipped(false)
{

}
SFXSoftbody::~SFXSoftbody()
{
	Clear();
}

void		SFXSoftbody::Clear()
{	
	for (unsigned int i=0; i<m_Parts.size(); i++)
	{
		MassPart* pPart = m_Parts[i];
		delete pPart;
	}
	for (unsigned int i=0; i<m_Springs.size(); i++)
	{
		Spring* pSpring = m_Springs[i];
		delete pSpring ;	
	}
	m_Springs.resize(0);
	m_Parts.resize(0);
	UTIL_SAFE_DELETEA(m_NodeA);

}

void	SFXSoftbody::Init(const std::string& Image, int Width, int Height, unsigned int Config, float Mass, float SpringStrength)
{
	Clear();
	m_Image.Create(Image);
	if (!m_Image.IsValid()) return;
	m_Image.GetScreenSize(m_Size);

	// setup the basic grid we'll use later
	m_Width = Width;
	m_Height = Height;
	m_NodeA = new MassPart*[m_Width * m_Height];
	memset(m_NodeA, 0, sizeof(MassPart*)*m_Width * m_Height);

	// generate the nodes
	for (int x = 0; x<m_Width; x++)
	for (int y = 0; y<m_Height; y++)
	{
		int idx = x + (y*m_Width);
		m_NodeA[idx] = AddMass(m_ScreenPos+GetGridPos(x, y));
	}

	// basic grid
	for (int x = 0; x<m_Width; x++)
	for (int y = 0; y<m_Height; y++)
	{
		MassPart*	pMe = GetMass(x, y);
		if (Config & SPRING_GRID)
		{
			AddSpring(GetMass(x-1, y), pMe);
			AddSpring(GetMass(x, y-1), pMe);
		}
		if (Config & SPRING_XBRACE)
		{
			AddSpring(GetMass(x-1, y-1), pMe);
			AddSpring(GetMass(x+1, y-1), pMe);
		}
		if (Config & SPRING_FLEXION)
		{
			AddSpring(GetMass(x-2, y), pMe, m_DefaultSpring*2.0f);
			AddSpring(GetMass(x, y-2), pMe, m_DefaultSpring*2.0f);
			AddSpring(GetMass(x-2, y-2), pMe, m_DefaultSpring*2.0f);
		}
	}
	// superstructure surrounding the object for extra stiffness
	if (Config & SPRING_SUPER)
	{
		MassPart* P1 = GetMass(0, 0);
		MassPart* P2 = GetMass(0, m_Height-1);
		MassPart* P3 = GetMass(m_Width-1, 0);
		MassPart* P4 = GetMass(m_Width-1, m_Height-1);

		AddSpring(P1, P2, SpringStrength * 2);
		AddSpring(P1, P4, SpringStrength * 2);
		AddSpring(P2, P4, SpringStrength * 2);
		AddSpring(P3, P4, SpringStrength * 2);

		// cross braces with extra power
		AddSpring(P1, P3, SpringStrength * 6);
		AddSpring(P2, P3, SpringStrength * 6);
	}
}

void	SFXSoftbody::Tick(float dt)
{
	// update the forces acting on the particles
	for (unsigned int i=0; i<m_Springs.size(); i++)
	{
		Spring* pSpring = m_Springs[i];
		math::Vec2	Dir = pSpring->pA->Pos - pSpring->pB->Pos;
		float t = (Dir).Magnitude();
		if (t < 0.0001f)  // avoid integrator blowing up on overlapping particles
		{
			Dir = math::Vec2(1, 0);
			t = 1.0;
		};
		Dir/=t;
		float	Force = (t-pSpring->DesiredLength) * pSpring->Force *0.5f;
		pSpring->pA->Speed -= (Dir * Force) / pSpring->pA->Mass;
		pSpring->pB->Speed += (Dir * Force) / pSpring->pB->Mass;
	}

	float	Floor = m_ScreenPos.y + GetGridPos(0, m_Height-1).y;
	float	scale = (float)exp(-dt / 0.2f);		// friction
	for (unsigned int i=0; i<m_Parts.size(); i++)
	{
		MassPart* pPart = m_Parts[i];
		pPart->Speed.y += m_DefaultGravity * dt;			// gravity
		pPart->Pos += pPart->Speed * dt;		// use a smarter integrator?
		if (pPart->Pos.y >Floor) pPart->Pos.y = Floor ;	// floor
		pPart->Speed *= scale;					// apply friction
	}
}

void	SFXSoftbody::Render(const math::Vec2& Offset, float Scale)
{
	if (m_Width == 1 || m_Height ==1)
		RenderRope(Offset, Scale);
	else
		RenderGrid(Offset, Scale);
}

void	SFXSoftbody::RenderRope(const math::Vec2& Offset, float Scale)
{
	Graphics::Vertex2D*	VertA = Graphics::GetTempVert();
	unsigned short*		IndexA = Graphics::GetTempIdx();

	bool			IsVertical = (m_Height == 1);
	unsigned int	Vertex = 0;
	unsigned int	Index = 0;
	// rig the rope using deltas
	float			Width = m_Size.y * 0.5f;
	int				Count = m_Height;
	math::Vec2		UV1 = m_Image.GetUV1();
	math::Vec2		UV2 = m_Image.GetUV2();
	if (IsVertical)	
	{
		Width = m_Size.x * 0.5f;
		Count = m_Width;
	}
	float	sx = Scale * GraphicsUtil::WIDTH;
	float	sy = Scale * GraphicsUtil::HEIGHT;
	for (int i=0; i<Count; i++)
	{
		math::Vec2	P1 = m_NodeA[i]->Pos;
		math::Vec2	P2 = m_NodeA[(i+1) % Count]->Pos;

		math::Vec2	Normal(P2.y-P1.y, -(P2.x-P1.x));
		Normal.Normalize();
		Normal*= Width;

		float	x1 = (P1.x - Normal.x - Offset.x) * sx;
		float	y1 = (P1.y - Normal.y - Offset.y) * sy;
		float	x2 = (P1.x + Normal.x - Offset.x) * sx;
		float	y2 = (P1.y + Normal.y - Offset.y) * sy;
		if (IsVertical)
		{
			float t = UV1.y + ((UV2.y-UV1.y) * (i / float(Count)));
			VertA[Vertex++].Fill(x1, y1, m_Color, UV1.x, t);
			VertA[Vertex++].Fill(x2, y2, m_Color, UV2.x, t);
		} else
		{
			float t = UV1.x + ((UV2.x-UV1.x) * (i / float(Count)));
			VertA[Vertex++].Fill(x1, y1, m_Color, t, UV1.y);
			VertA[Vertex++].Fill(x2, y2, m_Color, t, UV2.y);
		}
	}

	// build index list
	for (int i=0; i<Count-1; i++)
	{
		int TL = i * 2;
		IndexA[Index++] = TL;
		IndexA[Index++] = TL+1;
		IndexA[Index++] = TL+3;

		IndexA[Index++] = TL;
		IndexA[Index++] = TL+3;
		IndexA[Index++] = TL+2;
	}

	m_Image.SetTexture();
	GetGraphics()->DrawIndexVertex2D(VertA, Vertex, IndexA, Index);
}

void	SFXSoftbody::RenderGrid(const math::Vec2& Offset, float Scale)
{
	Graphics::Vertex2D*	VertA = Graphics::GetTempVert();
	unsigned short*		IndexA = Graphics::GetTempIdx();

	math::Vec2	P = -Offset;
	int	Index = 0, Vertex =0;
	math::Vec2	UV1 = m_Image.GetUV1();
	math::Vec2	UV2 = m_Image.GetUV2();

	if (m_IsFlipped)
	{
		UV2.x = UV1.x;
		UV1.x = m_Image.GetUV2().x;
	}
	math::Vec2	UVT = UV2-UV1;
	float		SX = GraphicsUtil::WIDTH * Scale;
	float		SY = GraphicsUtil::HEIGHT* Scale;

	COLOR Color = 0xFFFFFFFF;
	
	for (int x=0; x<m_Width; x++)
	for (int y=0; y<m_Height; y++)
	{
		MassPart*	pPart = GetMass(x, y);
		if (!pPart) continue;


		float		tx = x / (float)(m_Width-1);
		float		ty = y / (float)(m_Height-1);
		VertA[Vertex++].Fill((P.x + pPart->Pos.x)*SX, (P.y + pPart->Pos.y)*SY, m_Color, UV1.x + (UVT.x*tx), UV1.y + (UVT.y*ty));
	}
	
	for (int x=0; x<m_Width-1; x++)
	for (int y=0; y<m_Height-1; y++)
	{
		IndexA[Index++] = (x+0) + ((y+0)*m_Width);
		IndexA[Index++] = (x+1) + ((y+1)*m_Width);
		IndexA[Index++] = (x+1) + ((y+0)*m_Width);

		IndexA[Index++] = (x+0) + ((y+0)*m_Width);
		IndexA[Index++] = (x+0) + ((y+1)*m_Width);
		IndexA[Index++] = (x+1) + ((y+1)*m_Width);
	}

	m_Image.SetTexture();
	GetGraphics()->DrawIndexVertex2D(VertA, Vertex, IndexA, Index);
}

void	SFXSoftbody::RenderDebug(const math::Vec2& Offset, float Scale)
{
	dd::Manager& DD = dd::Manager::Instance();
	math::Vec2	P = -Offset;
	math::Vec2	Box(5.0f*GraphicsUtil::InvW, 5.0f*GraphicsUtil::InvH);
	for (unsigned int i=0; i<m_Parts.size(); i++)
	{
		MassPart* pPart = m_Parts[i];
		DD.AddLine2D((P + pPart->Pos - Box)*Scale, (P + pPart->Pos + Box)*Scale, ColorRGBA(255, 0, 0));
	}
	for (unsigned int i=0; i<m_Springs.size(); i++)
	{
		Spring* pSpring = m_Springs[i];
		DD.AddLine2D((P+pSpring->pA->Pos)*Scale, (P+pSpring->pB->Pos)*Scale, ColorRGBA(0, 255, 0));
	}
}

math::Vec2		SFXSoftbody::GetGridPos(int x, int y)
{
	return math::Vec2(-m_Size.x*0.5f, -m_Size.y * 0.7f) + math::Vec2(x * m_Size.x / float(m_Width), y * m_Size.y/ float(m_Height));
}


SFXSoftbody::MassPart*	SFXSoftbody::GetMass(int x, int y)
{
	if (!m_NodeA || x < 0 || y < 0 || x>=m_Width || y>=m_Height) return NULL;
	return m_NodeA[x + (y*m_Width)];

}
SFXSoftbody::MassPart*	SFXSoftbody::AddMass(const math::Vec2& P1)
{
	MassPart*	pMass = new MassPart;
	pMass->LastPos = pMass->Pos = P1;
	pMass->Mass = m_DefaultMass;
	m_Parts.push_back(pMass);
	return pMass;
}
SFXSoftbody::Spring*		SFXSoftbody::AddSpring(SFXSoftbody::MassPart* pA, SFXSoftbody::MassPart* pB, float SpringVal)
{
	if (!pA || !pB) return NULL;
	Spring*	pSpring = new Spring;
	pSpring->pA = pA;
	pSpring->pB = pB;
	if (SpringVal < 0)
	{
		pSpring->Force = m_DefaultSpring;
	} else
	{
		pSpring->Force = SpringVal;
	}
	pSpring->DesiredLength = pSpring->Length = (pA->Pos - pB->Pos).Magnitude();
	m_Springs.push_back(pSpring);
	return pSpring;
}

void		SFXSoftbody::UpdateAllSprings(float Strength)
{
	for (unsigned int i=0; i<m_Springs.size(); i++)
	{
		m_Springs[i]->Force = Strength;
	}
}